#include <covariance/property.h>
#include <covariance/visual.h>

#include <rviz/properties/color_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/properties/enum_property.h>

#include <QColor>

#include <OgreSceneManager.h>
#include <OgreSceneNode.h>

namespace mrs_rviz_plugins
{

namespace covariance
{

Property::Property(const QString& name, bool default_value, const QString& description, rviz::Property* parent, const char* changed_slot, QObject* receiver)
    // NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of this constructor
    : BoolProperty(name, default_value, description, parent) {

  position_property_ = new BoolProperty("Position", true, "Whether or not to show the position part of covariances", this, SLOT(updateVisibility()));
  position_property_->setDisableChildrenIfFalse(true);

  position_color_property_ = new rviz::ColorProperty("Color", QColor(204, 51, 204), "Color to draw the position covariance ellipse.", position_property_,
                                                     SLOT(updateColorAndAlphaAndScaleAndOffset()), this);

  position_alpha_property_ = new rviz::FloatProperty("Alpha", 0.3f, "0 is fully transparent, 1.0 is fully opaque.", position_property_,
                                                     SLOT(updateColorAndAlphaAndScaleAndOffset()), this);
  position_alpha_property_->setMin(0);
  position_alpha_property_->setMax(1);

  position_scale_property_ =
      new rviz::FloatProperty("Scale", 1.0f, "Scale factor to be applied to covariance ellipse. Corresponds to the number of standard deviations to display",
                              position_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this);
  position_scale_property_->setMin(0);

  orientation_property_ = new BoolProperty("Orientation", true, "Whether or not to show the orientation part of covariances", this, SLOT(updateVisibility()));
  orientation_property_->setDisableChildrenIfFalse(true);

  orientation_frame_property_ = new rviz::EnumProperty("Frame", "Local", "The frame used to display the orientation covariance.", orientation_property_,
                                                       SLOT(updateOrientationFrame()), this);
  orientation_frame_property_->addOption("Local", Local);
  orientation_frame_property_->addOption("Fixed", Fixed);

  orientation_colorstyle_property_ =
      new rviz::EnumProperty("Color Style", "Unique", "Style to color the orientation covariance: XYZ with same unique color or following RGB order",
                             orientation_property_, SLOT(updateColorStyleChoice()), this);
  orientation_colorstyle_property_->addOption("Unique", Unique);
  orientation_colorstyle_property_->addOption("RGB", RGB);

  orientation_color_property_ = new rviz::ColorProperty("Color", QColor(255, 255, 127), "Color to draw the covariance ellipse.", orientation_property_,
                                                        SLOT(updateColorAndAlphaAndScaleAndOffset()), this);

  orientation_alpha_property_ = new rviz::FloatProperty("Alpha", 0.5f, "0 is fully transparent, 1.0 is fully opaque.", orientation_property_,
                                                        SLOT(updateColorAndAlphaAndScaleAndOffset()), this);
  orientation_alpha_property_->setMin(0);
  orientation_alpha_property_->setMax(1);

  orientation_offset_property_ =
      new rviz::FloatProperty("Offset", 1.0f,
                              "For 3D poses is the distance where to position the ellipses representing orientation covariance. For 2D "
                              "poses is the height of the triangle representing the variance on yaw",
                              orientation_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this);
  orientation_offset_property_->setMin(0);

  orientation_scale_property_ = new rviz::FloatProperty(
      "Scale", 1.0f, "Scale factor to be applied to orientation covariance shapes. Corresponds to the number of standard deviations to display",
      orientation_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this);
  orientation_scale_property_->setMin(0);

  connect(this, SIGNAL(changed()), this, SLOT(updateVisibility()));

  // Connect changed() signal here instead of doing it through the initialization of BoolProperty().
  // We do this here to make changed_slot be called _after_ updateVisibility()
  if (changed_slot && (parent || receiver)) {
    if (receiver)
      connect(this, SIGNAL(changed()), receiver, changed_slot);
    else
      connect(this, SIGNAL(changed()), parent, changed_slot);
  }

  setDisableChildrenIfFalse(true);
}

Property::~Property() {
}

void Property::updateColorStyleChoice() {
  bool use_unique_color = (orientation_colorstyle_property_->getOptionInt() == Unique);
  orientation_color_property_->setHidden(!use_unique_color);
  updateColorAndAlphaAndScaleAndOffset();
}

void Property::updateColorAndAlphaAndScaleAndOffset() {
  D_Covariance::iterator it_cov  = covariances_.begin();
  D_Covariance::iterator end_cov = covariances_.end();
  for (; it_cov != end_cov; ++it_cov)
    updateColorAndAlphaAndScaleAndOffset(*it_cov);
}

void Property::updateColorAndAlphaAndScaleAndOffset(const VisualPtr& visual) {
  float  pos_alpha = position_alpha_property_->getFloat();
  float  pos_scale = position_scale_property_->getFloat();
  QColor pos_color = position_color_property_->getColor();
  visual->setPositionColor(pos_color.redF(), pos_color.greenF(), pos_color.blueF(), pos_alpha);
  visual->setPositionScale(pos_scale);

  float ori_alpha  = orientation_alpha_property_->getFloat();
  float ori_offset = orientation_offset_property_->getFloat();
  float ori_scale  = orientation_scale_property_->getFloat();
  if (orientation_colorstyle_property_->getOptionInt() == Unique) {
    QColor ori_color = orientation_color_property_->getColor();
    visual->setOrientationColor(ori_color.redF(), ori_color.greenF(), ori_color.blueF(), ori_alpha);
  } else {
    visual->setOrientationColorToRGB(ori_alpha);
  }
  visual->setOrientationOffset(ori_offset);
  visual->setOrientationScale(ori_scale);
}

void Property::updateVisibility() {
  D_Covariance::iterator it_cov  = covariances_.begin();
  D_Covariance::iterator end_cov = covariances_.end();
  for (; it_cov != end_cov; ++it_cov)
    updateVisibility(*it_cov);
}

void Property::updateVisibility(const VisualPtr& visual) {
  bool show_covariance = getBool();
  if (!show_covariance) {
    visual->setVisible(false);
  } else {
    bool show_position_covariance = position_property_->getBool();
    ;
    bool show_orientation_covariance = orientation_property_->getBool();
    visual->setPositionVisible(show_position_covariance);
    visual->setOrientationVisible(show_orientation_covariance);
  }
}

void Property::updateOrientationFrame() {
  D_Covariance::iterator it_cov  = covariances_.begin();
  D_Covariance::iterator end_cov = covariances_.end();
  for (; it_cov != end_cov; ++it_cov)
    updateOrientationFrame(*it_cov);
}

void Property::updateOrientationFrame(const VisualPtr& visual) {
  bool use_rotating_frame = (orientation_frame_property_->getOptionInt() == Local);
  visual->setRotatingFrame(use_rotating_frame);
}

void Property::popFrontVisual() {
  covariances_.pop_front();
}

void Property::clearVisual() {
  covariances_.clear();
}

size_t Property::sizeVisual() {
  return covariances_.size();
}

Property::VisualPtr Property::createAndPushBackVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node) {
  bool      use_rotating_frame = (orientation_frame_property_->getOptionInt() == Local);
  VisualPtr visual(new Visual(scene_manager, parent_node, use_rotating_frame));
  updateVisibility(visual);
  updateOrientationFrame(visual);
  updateColorAndAlphaAndScaleAndOffset(visual);
  covariances_.push_back(visual);
  return visual;
}

bool Property::getPositionBool() {
  return position_property_->getBool();
}

bool Property::getOrientationBool() {
  return orientation_property_->getBool();
}

}  // namespace covariance

}  // namespace mrs_rviz_plugins
